
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
// import edu.wpi.first.wpilibj.templates.commands.DriveInASquare_CG;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.BallFeederStart;
// import edu.wpi.first.wpilibj.templates.commands.DriveInACircle;
import edu.wpi.first.wpilibj.templates.commands.ToggleController;
import edu.wpi.first.wpilibj.templates.commands.CatapultShoot;
 import edu.wpi.first.wpilibj.templates.commands.DriveStraight;
import edu.wpi.first.wpilibj.templates.commands.ResetEncoders;
import edu.wpi.first.wpilibj.templates.commands.TimedFeeder;
import edu.wpi.first.wpilibj.templates.commands.TimedShootCatapult_CG;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    
    
    //Buttons
    private Joystick drivingStick;
    private Joystick secondaryStick;
    private JoystickButton squareDriveButton;// One type of button is a joystick button which is any button on a joystick.
    private JoystickButton circleDriveButton;
    //private JoystickButton pneumaticToggleButton;
    //private JoystickButton pneumaticTimedButton;
    //private JoystickButton feederToggleButton;
    //private JoystickButton feederTimedUpButton;
    //private JoystickButton feederTimedDownButton;
    private JoystickButton catapultShootButton;
    private JoystickButton catapultTimedShootButton;
    private JoystickButton catapultOnButton;
    private JoystickButton catapultOffButton;
    private JoystickButton feederToggleButton;
    private JoystickButton feederTimedUpButton;
    private JoystickButton feederTimedDownButton;
    private JoystickButton feederOnButton;
    private JoystickButton feederOffButton;
    private JoystickButton toggleControllerButton;
    private JoystickButton randomFunctionButton;
    private JoystickButton resetEncoders;
    
    //private JoystickButton randomFunctionButton;
    
    //Controller Boolean
    private boolean isJoystick = true; //False for X-Box Controller
    
    public OI(){
        drivingStick = new Joystick(RobotMap.JOYSTICK_1);
        secondaryStick = new Joystick(RobotMap.JOYSTICK_2);
        SmartDashboard.putData(new ToggleController());
        
        //Joystick Controls
        //Driving Joystick Buttons
        /*squareDriveButton = new JoystickButton(drivingStick, 9); //X-Box Controller
        //squareDriveButton.whenPressed(new DriveInASquare_CG()); 
        //circleDriveButton = new JoystickButton(drivingStick, 10); //X-Box Controller
        //circleDriveButton.whenPressed(new DriveInACircle());*/

        //Misc. Joystick Buttons
        catapultTimedShootButton = new JoystickButton(secondaryStick, 3);
        //catapultTimedShootButton.whenPressed(new TimedShootCatapult_CG());
        feederTimedUpButton = new JoystickButton(secondaryStick, 4);
        //feederTimedUpButton.whenPressed(new TimedFeeder(5, 0));
        feederTimedDownButton = new JoystickButton(secondaryStick, 5);
        //feederTimedDownButton.whenPressed(new BallFeederStart());
        toggleControllerButton = new JoystickButton(drivingStick, 8);
        toggleControllerButton.whenPressed(new ToggleController());
        resetEncoders = new JoystickButton(drivingStick, 11);
        //resetEncoders.whenPressed(new ResetEncoders());
        
        /*pneumaticToggleButton = new JoystickButton(cameraStick, Joystick.ButtonType.kTop.value);
        pneumaticToggleButton.whenPressed(new TogglePiston());
        pneumaticTimedButton = new JoystickButton(cameraStick, 3);
        pneumaticTimedButton.whenPressed(new TimedPiston());
        feederToggleButton = new JoystickButton(cameraStick, 2);
        feederToggleButton.whenPressed();*/
        /*feederTimedUpButton = new JoystickButton(cameraStick, 4);
        feederTimedUpButton.whenPressed(new TimedFeeder(5, 0)); //To-Do: Get from Smart Dashboard
        feederTimedDownButton = new JoystickButton(cameraStick, 5);
        feederTimedDownButton.whenPressed(new TimedFeeder(5, 1)); //To-Do: Get from Smart Dashboard*/
        randomFunctionButton = new JoystickButton(drivingStick, 8);
        //randomFunctionButton.whenPressed(new DriveStraight(3.0));
        
    }
    
    public Joystick getDriveStick(){
        return drivingStick;
    }
    
    public Joystick getCamStick() {
        return secondaryStick;
    }
    
     public Joystick getSecondaryStick() {
        return secondaryStick;
    }
    
    public boolean isJoystick(){
        return isJoystick;
    }
    
    public void setJoystick(boolean iJ){
        isJoystick = iJ;
    }
    
   /* public void changeControllerLayout(boolean iJ){
        if(iJ){
             //Joystick Controls
            //SmartDashboard.putString("Controller Type: ", "Joystick");
            //Joystick Controls
            //Driving Joystick Buttons
            //squareDriveButton = new JoystickButton(drivingStick, 9); //X-Box Controller
            //squareDriveButton.whenPressed(new DriveInASquare_CG()); 
            //circleDriveButton = new JoystickButton(drivingStick, 10); //X-Box Controller
            //circleDriveButton.whenPressed(new DriveInACircle());

            //Misc. Joystick Buttons
            //catapultShootButton = new JoystickButton(cameraStick, 1);
            //catapultShootButton.whenPressed(new CatapultShoot());
            //feederTimedUpButton = new JoystickButton(cameraStick, 4);
            //feederTimedUpButton.whenPressed(new TimedFeeder(5, 0));
            //feederTimedDownButton = new JoystickButton(cameraStick, 5);
            //feederTimedDownButton.whenPressed(new TimedFeeder(5, 1));
            //toggleControllerButton = new JoystickButton(cameraStick, 8);
            //toggleControllerButton.whenPressed(new ToggleController());
            //Driving Joystick Buttons
            /*squareDriveButton = new JoystickButton(drivingStick, 9); //X-Box Controller
            squareDriveButton.whenPressed(new DriveInASquare_CG()); 
            circleDriveButton = new JoystickButton(drivingStick, 10); //X-Box Controller
            circleDriveButton.whenPressed(new DriveInACircle());*/

            //Misc. Joystick Buttons
            /*pneumaticToggleButton = new JoystickButton(cameraStick, Joystick.ButtonType.kTop.value);
            pneumaticToggleButton.whenPressed(new TogglePiston());
            pneumaticTimedButton = new JoystickButton(cameraStick, 3);
            pneumaticTimedButton.whenPressed(new TimedPiston());
            feederToggleButton = new JoystickButton(cameraStick, 2);
            feederToggleButton.whenPressed();
            feederTimedUpButton = new JoystickButton(cameraStick, 4);
            feederTimedUpButton.whenPressed(new TimedFeeder(5, 0)); //To-Do: Get from Smart Dashboard
            feederTimedDownButton = new JoystickButton(cameraStick, 5);
            feederTimedDownButton.whenPressed(new TimedFeeder(5, 1)); //To-Do: Get from Smart Dashboard
        }else{
            //X-Box Controller
            SmartDashboard.putString("Controller Type: ", "X-Box");
            //Joystick Controls
            //Driving Joystick Buttons
            /*squareDriveButton = new JoystickButton(drivingStick, 9); //X-Box Controller
            squareDriveButton.whenPressed(new DriveInASquare_CG()); 
            circleDriveButton = new JoystickButton(drivingStick, 10); //X-Box Controller
            circleDriveButton.whenPressed(new DriveInACircle());

            //Misc. Joystick Buttons
            catapultShootButton = new JoystickButton(drivingStick, 6);
            catapultShootButton.whenPressed(new CatapultShoot());
            feederTimedUpButton = new JoystickButton(drivingStick, 3);
            feederTimedUpButton.whenPressed(new TimedFeeder(5, 0));
            feederTimedDownButton = new JoystickButton(drivingStick, 4);
            feederTimedDownButton.whenPressed(new TimedFeeder(5, 1));
            toggleControllerButton = new JoystickButton(drivingStick, 8);
            toggleControllerButton.whenPressed(new ToggleController());
            //Driving Joystick Buttons
            //squareDriveButton = new JoystickButton(drivingStick, 9); //X-Box Controller
            //squareDriveButton.whenPressed(new DriveInASquare_CG()); 
            //circleDriveButton = new JoystickButton(drivingStick, 10); //X-Box Controller
            //circleDriveButton.whenPressed(new DriveInACircle());

            //Misc. Joystick Buttons
            //pneumaticToggleButton = new JoystickButton(cameraStick, 3);
            //pneumaticToggleButton.whenPressed(new TogglePiston());
            //pneumaticTimedButton = new JoystickButton(cameraStick, 2);
            //pneumaticTimedButton.whenPressed(new TimedPiston());
            //feederToggleButton = new JoystickButton(cameraStick, 2);
            //feederToggleButton.whenPressed();
            /*feederTimedUpButton = new JoystickButton(cameraStick, 4);
            feederTimedUpButton.whenPressed(new TimedFeeder(5, 0)); //To-Do: Get from Smart Dashboard
            feederTimedDownButton = new JoystickButton(cameraStick, 5);
            feederTimedDownButton.whenPressed(new TimedFeeder(5, 1)); //To-Do: Get from Smart Dashboard
        //}
    }*/
    
    public double getMagnitude(){
        double Magnitude = 0;
        Magnitude = drivingStick.getMagnitude();
        return Magnitude;
    }
    
    public double getDegrees(){
        double Degrees = 0;
        Degrees = drivingStick.getDirectionDegrees();
        return Degrees;
    }
    
    public double getTwist(){
        double Twist = 0;
        if(isJoystick){
            Twist = secondaryStick.getX();
        }else{
            Twist = drivingStick.getRawAxis(4);
        }
        if(Twist < 0.1 && Twist > -0.1){
            Twist = 0;      
        }
        //Twist /= 5;
        return Twist;
    }
}
